/*
 * util.cpp
 *
 * Created: 29.10.2013 19:56:58
 *  Author: Elektronik
 */ 

#include "util.h"

//==================================================================================

OPTIMIZE_O3 uint16_t calc_checkum16(uint8_t *data, uint8_t bytes)
{
	uint16_t checkum;
	
	asm volatile(
	" ser	%A[sum]				\n\t"	// compute checkum
	" ser	%B[sum]				\n\t"
	" repeat_%=:				\n\t"
	" ld	r0, Z+				\n\t"
	" add	%A[sum], r0			\n\t"
	" adc	%A[sum], r1			\n\t"
	" add	%B[sum], %A[sum]	\n\t"
	" adc	%B[sum], r1			\n\t"
	" dec	%[count]			\n\t"
	" brne	repeat_%=			\n\t"
	
	" ser	%[count]			\n\t"	// calc checkum with which is checkum is 0
	" sub	%[count], %B[sum]	\n\t"
	" cp	%[count], %A[sum]	\n\t"
	" sbc	%[count], r1		\n\t"
	" sub	%[count], %A[sum]	\n\t"
	" add	%A[sum], %[count]	\n\t"
	" adc	%A[sum], r1			\n\t"
	
	" ser	%B[sum]				\n\t"
	" sub	%B[sum], %A[sum]	\n\t"
	" mov	%A[sum], %[count]	\n\t"
	: [sum] "=&r" (checkum)
	: "z" (data), [count] "d" (bytes) );
	
	return checkum;
}